// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (Robotics and Perception Group, University of Zurich, Switzerland).
//
// SVO is free software: you can redistribute it and/or modify it under the
// terms of the GNU General Public License as published by the Free Software
// Foundation, either version 3 of the License, or any later version.
//
// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.

#ifndef SVO_POINT_H_
#define SVO_POINT_H_

#include <boost/noncopyable.hpp>
#include <svo/global.h>

namespace g2o {
	class VertexSBAPointXYZ;
}
typedef g2o::VertexSBAPointXYZ g2oPoint;

namespace svo {

	class Feature;

	typedef Matrix<double, 2, 3> Matrix23d;

	/// A 3D point on the surface of the scene.
	class Point : boost::noncopyable
	{
	public:
		EIGEN_MAKE_ALIGNED_OPERATOR_NEW

			enum PointType {
			TYPE_DELETED,
			TYPE_CANDIDATE,
			TYPE_UNKNOWN,
			TYPE_GOOD
		};

		static int                  point_counter_;           //!< Counts the number of created points. Used to set the unique id.
		int                         id_;                      //!< Unique ID of the point.
		Vector3d                    pos_;                     //!< 3d pos of the point in the world coordinate frame.
		Vector3d                    normal_;                  //!< Surface normal at point.
		Matrix3d                    normal_information_;      //!< Inverse covariance matrix of normal estimation.
		bool                        normal_set_;              //!< Flag whether the surface normal was estimated or not.
		list<Feature*>              obs_;                     //!< References to keyframes which observe the point.
		size_t                      n_obs_;                   //!< Number of obervations: Keyframes AND successful reprojections in intermediate frames.
		g2oPoint*                   v_pt_;                    //!< Temporary pointer to the point-vertex in g2o during bundle adjustment.
		int                         last_published_ts_;       //!< Timestamp of last publishing.
		int                         last_projected_kf_id_;    //!< Flag for the reprojection: don't reproject a pt twice.
		PointType                   type_;                    //!< Quality of the point.
		int                         n_failed_reproj_;         //!< Number of failed reprojections. Used to assess the quality of the point.
		int                         n_succeeded_reproj_;      //!< Number of succeeded reprojections. Used to assess the quality of the point.
		int                         last_structure_optim_;    //!< Timestamp of last point optimization

		Point(const Vector3d& pos);
		Point(const Vector3d& pos, Feature* ftr);
		~Point();

		/// Add a reference to a frame.
		void addFrameRef(Feature* ftr);

		/// Remove reference to a frame.
		bool deleteFrameRef(Frame* frame);

		/// Initialize point normal. The inital estimate will point towards the frame.
		void initNormal();

		/// Check whether mappoint has reference to a frame.
		Feature* findFrameRef(Frame* frame);

		/// Get Frame with similar viewpoint.
		bool getCloseViewObs(const Vector3d& pos, Feature*& obs) const;

		/// Get number of observations.
		inline size_t nRefs() const { return obs_.size(); }

		/// Optimize point position through minimizing the reprojection error.
		void optimize(const size_t n_iter);

		/// Jacobian of point projection on unit plane (focal length = 1) in frame (f).
		inline static void jacobian_xyz2uv(
			const Vector3d& p_in_f,
			const Matrix3d& R_f_w,
			Matrix23d& point_jac)
		{
			const double z_inv = 1.0 / p_in_f[2];
			const double z_inv_sq = z_inv*z_inv;
			point_jac(0, 0) = z_inv;
			point_jac(0, 1) = 0.0;
			point_jac(0, 2) = -p_in_f[0] * z_inv_sq;
			point_jac(1, 0) = 0.0;
			point_jac(1, 1) = z_inv;
			point_jac(1, 2) = -p_in_f[1] * z_inv_sq;
			point_jac = -point_jac * R_f_w;
		}
	};

} // namespace svo

#endif // SVO_POINT_H_
